#include "trdp.hpp"
#include <unordered_map>

using namespace trdp;

std::string global_local_ip;
uint8_t global_local_ip_int[4];
std::string global_multi_ip;
uint8_t global_multi_ip_int[4];

// to get buffer struct pointer by com id, used by pd_sub_callback function
std::unordered_map<uint32_t, buffer_t *> global_buffer_map;

int trdp::init(std::string local_ip, std::string multi_ip)
{
	const char *ifp[1];
	const char *mcp[1];
	ifp[0] = local_ip.c_str();
	mcp[0] = multi_ip.c_str();
	global_local_ip = local_ip;
	global_multi_ip = multi_ip;
	int nums[4];
	sscanf(global_local_ip.c_str(), "%d.%d.%d.%d", nums, nums + 1, nums + 2, nums + 3);
	for (int i = 0; i < 4; i++)
	{
		global_local_ip_int[i] = nums[i];
	}

	sscanf(global_multi_ip.c_str(), "%d.%d.%d.%d", nums, nums + 1, nums + 2, nums + 3);
	for (int i = 0; i < 4; i++)
	{
		global_multi_ip_int[i] = nums[i];
	}
	printf("trdpmini local %d.%d.%d.%d, multi %d.%d.%d.%d\n", global_local_ip_int[0],
		   global_local_ip_int[1], global_local_ip_int[2], global_local_ip_int[3],
		   global_multi_ip_int[0], global_multi_ip_int[1], global_multi_ip_int[2],
		   global_multi_ip_int[3]);
	return trdp_mini_init_so(1, 1, ifp, mcp, 2, 128, COM_UDP_MULTICAST);
}

TRDP_MINI_ERR_T pd_sub_callback(void *arg, uint32_t comid, uint8_t *pdata, uint32_t datasize,
								uint8_t *recvip, uint16_t recvport, TRDP_MINI_ERR_T errcode)
{
	// 根据comid得到缓冲区，将数据存入缓冲区
	if (global_buffer_map.find(comid) == global_buffer_map.end())
	{
		// if the key is not found
		fprintf(stderr, "received key 0x%x not found in map\n", comid);
		return TRDP_MINI_NO_ERR;
	}
	buffer_t *buf = global_buffer_map[comid];
	if (datasize > buf->size)
	{
		fprintf(stderr, "received data size too large (%d got, %d max)\n", datasize, buf->size);
		return TRDP_MINI_NO_ERR;
	}
	memcpy(buf->data, pdata, datasize);
	buf->len = datasize;
	return TRDP_MINI_NO_ERR;
}

/**
 * @brief create an object, initialize buffer structs, subscribe the com id. on failure, throw a
 * string
 *
 * @param com_id com id to subscribe
 * @param data_size size of the process data of this com id
 */
pd_sub::pd_sub(uint32_t com_id, uint32_t data_size)
{
	// alloc a buffer struct, insert into map
	this->buffer = (buffer_t *)malloc(sizeof(buffer_t));
	if (this->buffer == NULL) throw TRDP_MINI_MEM_ERR;
	;
	uint8_t *databuf = (uint8_t *)malloc(data_size);
	if (databuf == NULL)
	{
		free(this->buffer);
		throw TRDP_MINI_MEM_ERR;
	}
	this->buffer->comid = com_id;
	this->buffer->data = databuf;
	this->buffer->size = data_size;
	this->buffer->len = 0;
	global_buffer_map.insert({com_id, this->buffer});
	// subscribe with the callback

	auto err = trdp_mini_pd_subscribe(com_id, global_local_ip_int, global_multi_ip_int,
									  COM_UDP_MULTICAST, pd_sub_callback);
	if (err < 0)
	{
		free(this->buffer);
		free(databuf);
		throw err;
	}
}

/**
 * @brief destroy the object, unsubscribe and free buffer
 *
 */
pd_sub::~pd_sub()
{
	// unsubscribe
	trdp_mini_pd_unsubscribe(this->buffer->comid);
	// delete map key and free buffer
	global_buffer_map.erase(this->buffer->comid);
	free(this->buffer->data);
	free(this->buffer);
}

/**
 * @brief copy received data to user supplied buffer
 *
 * @param databuf buffer to hold the data
 * @param bufsize size of the buffer
 * @return uint32_t 0: no fresh data, >0: got fresh data, is the size of the data
 */
uint32_t pd_sub::update(uint8_t *databuf, uint32_t bufsize)
{
	if (this->buffer->len == 0)
	{
		// no fresh data, return 0
		return 0;
	}
	if (this->buffer->len > bufsize)
	{
		// buffer too small
		fprintf(stderr, "failed to update process data, supplied buffer too small\n");
		return 0;
	}
	memcpy(databuf, this->buffer->data, this->buffer->len);
	auto ret = this->buffer->len;
	this->buffer->len = 0;
	return ret;
}

/**
 * @brief create an object, alloc buffer and prepare to publish, on failure, throw error code
 *
 * @param com_id com id
 * @param data_size process data size of the com id
 */
pd_pub::pd_pub(uint32_t com_id, uint32_t data_size)
{
	this->buffer = (buffer_t *)malloc(sizeof(buffer_t));
	this->buffer->comid = com_id;
	INT8U *temp = (INT8U *)malloc(data_size);
	memset(temp, 0x00, data_size);
	auto err = trdp_mini_pd_prepare(com_id, temp, data_size, global_local_ip_int,
									global_multi_ip_int, 0, 1000, COM_UDP_MULTICAST);
	free(temp);
	if (err < 0)
	{
		free(this->buffer);
		throw err;
	}
}

/**
 * @brief destroy the object, unprepare and free buffer
 *
 */
pd_pub::~pd_pub()
{
	trdp_mini_pd_unprepare(this->buffer->comid, global_multi_ip_int);
	free(this->buffer);
}

/**
 * @brief publish process data to the com id
 *
 * @param databuf buffer of the data
 * @param datasize size of the data
 * @return uint32_t 0: failed to put, >0: success
 */
uint32_t pd_pub::update(uint8_t *databuf, uint32_t datasize)
{
	auto err = trdp_mini_pd_put(this->buffer->comid, databuf, datasize, global_local_ip_int,
								global_multi_ip_int);
	if (err < 0)
		return 0;
	else
		return datasize;
}
